#include <time.h>
#include <sys/timerfd.h>

#include "IODrive.h"

MatVector frame_v[5];
int framev_index = 0;
SafeQueue <MatVector *> frame_que;
#if(CAMER_NUM == 2)
IODrive::IODrive():capture("/dev/video0", 2),capture_2("/dev/video1", 2)
#else
IODrive::IODrive():capture("/dev/video0", 2)
#endif
{
    fps = 0;
    frame_v[0].isusing = false;
    frame_v[1].isusing = false;
    frame_v[2].isusing = false;
    frame_v[3].isusing = false;
    frame_v[4].isusing = false;

#if(CAMER_NUM == 2)
    capture_2.setVideoFormat(WIDTH,HEIGHT, 1);  
    capture_2.setExposureTime(0, ExposureTime);//settings->exposure_time);
    capture_2.startStream();
    capture_2.info();

#endif
    capture.setVideoFormat(WIDTH,HEIGHT, 1);
    capture.setExposureTime(0, ExposureTime);//settings->exposure_time);
    capture.startStream();
    capture.info();


    #ifdef USING_UART2
    Uart2 = new SerialPort(&reactor,UART2_NAME,UART2_BAUD);
    #endif // USE_UART2

    #ifdef USING_UART3
    Uart3 = new SerialPort(&reactor,UART3_NAME,UART3_BAUD);
    #endif // USE_UART3
}

//#define DEBUG_VIDIOUPDATA
void IODrive::updataVidio(int fd)
{
    #ifdef DEBUG_VIDIOUPDATA
    double t = 0.0;
    #endif
    uint64_t exp;//该值为定时器超时次数
    read(fd, &exp, sizeof(uint64_t));
    if(!frame_v[framev_index].isusing)
    {
        #ifdef DEBUG_VIDIOUPDATA
            t = (double)cvGetTickCount();
        #endif
        #if(CAMER_NUM == 1)
            capture >> frame_v[framev_index].frame;
            if(frame_v[framev_index].frame.empty())
            {
                cout << "failed to load frame" << endl;
                return;
            }
        #elif(CAMER_NUM == 2)
			if(enableCamer_1)
			{
				capture >> frame_v[framev_index].frame_1;
				if(frame_v[framev_index].frame_1.empty())
				{
					cout << "failed to load frame" << endl;
					return;
				}
			}
			if(enableCamer_2)
			{
				capture_2 >> frame_v[framev_index].frame_2;
				if(frame_v[framev_index].frame_2.empty())
				{
					cout << "failed to load frame" << endl;
					return;
				}
			}

        #endif


        #ifdef DEBUG_VIDIOUPDATA
            t = (double)cvGetTickCount() - t;
            printf("time = %gms\n", t / ((double)cvGetTickFrequency()*1000.));
        #endif
        frame_v[framev_index].isusing = true;
        frame_que.push(&frame_v[framev_index]);
        fps++;
        framev_index++;
        framev_index %= 5;
    }
    else//当前空间正被占用，说明数据处理线程还未处理之前的数据
    {

    }
    /* 重置定时器时间　*/
    struct itimerspec curr_value;
    struct itimerspec new_value;
    timerfd_gettime(fd, &curr_value);
    new_value.it_interval = curr_value.it_interval;
    new_value.it_value = curr_value.it_interval;
    timerfd_settime(fd, 0, &new_value, NULL);
}

